// Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pcl_utils.h"
#include <cstddef>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <rerun/datatypes/vec3d.hpp>
#include <string>

template<typename PointCloudPtr>
int load_pcd_impl(const char *file_name, size_t name_len, PointCloudPtr cloud, size_t *size) {
    int ret = pcl::io::loadPCDFile(std::string(file_name, name_len), *cloud);
    *size = cloud->size();
    return ret;
}

extern "C" {
pcl::PointCloud<pcl::PointXYZ>* new_xyz_point_cloud() {
    return new pcl::PointCloud<pcl::PointXYZ>;
}

pcl::PointCloud<pcl::PointXYZI>* new_xyzi_point_cloud() {
    return new pcl::PointCloud<pcl::PointXYZI>;
}

void delete_xyz_point_cloud(pcl::PointCloud<pcl::PointXYZ> *cloud) {
    delete cloud;
}

void delete_xyzi_point_cloud(pcl::PointCloud<pcl::PointXYZI> *cloud) {
    delete cloud;
}

int load_xyz_pcd_file(char *file_name, size_t name_len, pcl::PointCloud<pcl::PointXYZ> *cloud, size_t *size) {
    return load_pcd_impl(file_name, name_len, cloud, size);
}

int load_xyzi_pcd_file(char *file_name, size_t name_len, pcl::PointCloud<pcl::PointXYZI> *cloud, size_t *size) {
    return load_pcd_impl(file_name, name_len, cloud, size);
}

void cloud_xyz_to_vecs3d(pcl::PointCloud<pcl::PointXYZ> *cloud, rerun::datatypes::Vec3D *vecs) {
    for (size_t i = 0; i < cloud->size(); i++) {
        const pcl::PointXYZ &point = cloud->points[i];
        vecs[i] = rerun::datatypes::Vec3D {point.x, point.y, point.z};
    }
}

void cloud_xyzi_to_vecs3d(pcl::PointCloud<pcl::PointXYZI> *cloud, float *vec_with_i) {
    for (size_t i = 0; i < cloud->size(); i++) {
        const pcl::PointXYZI &point = cloud->points[i];
        vec_with_i[i * 4 + 0] = point.x;
        vec_with_i[i * 4 + 1] = point.y;
        vec_with_i[i * 4 + 2] = point.z;
        vec_with_i[i * 4 + 3] = point.intensity;
    }
}

void cloud_xyzi_to_position3d(pcl::PointCloud<pcl::PointXYZI> *cloud, rerun::datatypes::Vec3D *position3d) {
    for (size_t i = 0; i < cloud->size(); i++) {
        const pcl::PointXYZI &point = cloud->points[i];
        position3d[i] = rerun::datatypes::Vec3D {point.x, point.y, point.z};
    }
}

void cloud_xyzi_to_intensity(pcl::PointCloud<pcl::PointXYZI> *cloud, float *intensity) {
    for (size_t i = 0; i < cloud->size(); i++) {
        const pcl::PointXYZI &point = cloud->points[i];
        intensity[i] = point.intensity;
    }
}
}